//
// Created by ZhaoXiaoFei on 2022/7/17.
//

#ifndef VINS_ESTIMATOR_FEATURE_MANAGER_HPP
#define VINS_ESTIMATOR_FEATURE_MANAGER_HPP

#include <eigen3/Eigen/Dense>
#include <vector>
#include <list>
#include <map>
#include <glog/logging.h>


class FeaturePerFrame{
public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1>& data){
        point.x() = data(0);
        point.y() = data(1);
        point.z() = data(2);
        uv.x() = data(3);
        uv.y() = data(4);
        vel_xy.x() = data(5);
        vel_xy.y() = data(6);
    }
    Eigen::Vector3d point;
    Eigen::Vector2d uv;
    Eigen::Vector2d vel_xy;
};

class FeaturesPerFeatureId{
public:
    FeaturesPerFeatureId(int featureId_, int start_frame_) : featurId(featureId_){
        start_frame = start_frame_;
        used_num = 0;
        estimated_depth = -1;
        solve_flag = 0;
    }
    const int featurId;
    int start_frame;
    int used_num;
    double estimated_depth;
    std::vector<FeaturePerFrame> features_per_id;
    int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;

    int endFrame(){
        return start_frame + features_per_id.size() - 1;
    }
};

class FeatureManager{
public:
    FeatureManager(const Eigen::Matrix3d* Rs_) : Rs(Rs_){

    }
    bool addFeatureAndCheckParallax(int frame_count, const std::map<int, std::vector<std::pair<int, Eigen::Matrix<double, 7, 1>>>>& image);
    int getFeatureCount();
    void clearState();
    Eigen::VectorXd getDepthVector();
    void setDepthVector(const Eigen::VectorXd& x);
    double computeParallax(FeaturesPerFeatureId& it, int frame_count);
    void removeOld();
    void removeOldAndShiftDepth(Eigen::Matrix3d& R1, Eigen::Vector3d& T1, Eigen::Matrix3d& R2, Eigen::Vector3d& T2);
    inline void setRbc(Eigen::Matrix3d& rbc){
        this->R_bc = rbc;
    }
    void removeFailures();
    void triangulation(Eigen::Vector3d Ps[], Eigen::Vector3d& tbc, Eigen::Matrix3d& rbc);
    void clearDepth(Eigen::VectorXd& depthVec);
    void removeSecondNew(int frame_count);
    std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> getCorresponding(int frameIdL, int frameIdR);
    std::list<FeaturesPerFeatureId> allFeatures;
    const Eigen::Matrix3d* Rs;
    Eigen::Matrix3d R_bc;
};

#endif //VINS_ESTIMATOR_FEATURE_MANAGER_HPP
